#include "controllerMissile.h"


void ControllerMissile::setTarget(int _targetId,int _targetLayer){
	targetId = _targetId;
	targetLayer = _targetLayer;
}

void ControllerMissile::recalc(int _object, int _layer, double _seconds_elapsed){
	
	//objecto a seguir
	if(targetId == 0) targetId = 0;
	if(targetLayer == 0) targetLayer = 1;
	Scene * scene = Scene::getScene();
	Vector3 myPos = *scene->getInstance(_object,_layer)->model * Vector3(0,0,0);
	Vector3 targetPos = *scene->getInstance(targetId,targetLayer)->model * Vector3(0,0,0);

	//float speed_enemy = scene->getInstance(targetId,targetLayer)->speed/1.5;
	
	float distance = abs(targetPos.x - myPos.x) +  abs(targetPos.y - myPos.y) +  abs(targetPos.z - myPos.z);

	//objecto a mover
	Matrix44 * m = scene->getInstance(_object,_layer)->model;
	Vector3 f = m->frontVector();
	Vector3 r = m->rightVector();
	Vector3 u = m->topVector();
	Vector3 p = *m * Vector3(0,0,0);

	float vel_ang = ((float)_seconds_elapsed * 1.0f);

	//PLANO IZQUIERDA/DERECHA
	float A = r.x;
	float B = r.y;
	float C = r.z;
	float D = -(r.x*p.x + r.y*p.y + r.z*p.z) ;
	
	float res = A*targetPos.x +B*targetPos.y +C*targetPos.z + D;
	

	int mov = 1;
	if (distance < 100) 
		mov = -1;

	if (res > 0){
		//doblar derecha
			m->rotateLocal(vel_ang,Vector3(0,mov,0));
	}
	else if (res < 0) {
		//doblar izquierda
			m->rotateLocal(vel_ang,Vector3(0,-mov,0));
	}
	
	//PLANO ARRIBA/ABAJO
	A = u.x;
	B = u.y;
	C = u.z;
	D =-( u.x*p.x + u.y*p.y + u.z*p.z) ;
	
	res = A*targetPos.x + B*targetPos.y + C*targetPos.z + D;
	
	if (res > 0){
		//SUBIR
		m->rotateLocal(vel_ang,Vector3(-mov,0,0));
	}
	else if (res < 0) {
		m->rotateLocal(vel_ang,Vector3(mov,0,0));
	}
	//hacer que quede el up igual del que persigue
	if(targetPos.dot(p) > 0){
		m->rotateLocal(vel_ang/10,Vector3(0,0,1));
	}
	else if (targetPos.dot(p) < 0){
		m->rotateLocal(vel_ang/10,Vector3(0,0,-1));
	}
	
	m->traslateLocal(0.0f,0.0f,-(float)_seconds_elapsed * 100.0f);

}
